#include "Crowd.h"
#include "vec.h"
#include <cmath>
#include <iostream>

Crowd::Crowd()
{
	neighborDist = 200;
	avoidWeight = 0.1f;
	headingWeight = 0.125f;
	centroidWeight = 0.01f;
	targetWeight = 0.1f;

	ignoreY = true;
}

void Crowd::tick()
{	
	//float *headingVel;
	//float *centroidVel;
	//float *avoidVel;
	//float *targetVel;
	
	float *newpos;
	float temp[3] = {0, 0, 0};
	float acc[3] = {0, 0, 0};
	float newacc[3] = {0, 0, 0};
	float magnitude = 0.1f;
	float weight = 0.1f;

	int i;

	for (i = 0; i < boids.size(); i++)
	{
		for (int k = 0; k < boids.size(); k++)
		{
			if (i != k)
			{
				float ad = 0, tti = 0;
				float p[3] = {boids[k]->pos(X), boids[k]->pos(Y), boids[k]->pos(Z)};
				float pv[3] = {boids[k]->_v[0], boids[k]->_v[1], boids[k]->_v[2]};
				//std::cerr << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << "\n";
				//std::cerr << "boid #" << k << "'s effect on boid#" << i << "\n";
				computeAlphaDerivative(boids[i], p, pv, ad, tti);
				//std::cerr << "alpha_g = " << ad << ",  tti_g = " << tti << "\n";
				//std::cerr << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << "\n";
			}
		}
	}

	for (i = 0; i < boids.size(); i++)
	{
		//centroidVel = NULL;
		//headingVel = NULL;
		//avoidVel = NULL;
		//targetVel = NULL;
		
		//Compute velocities
		//centroidVel = computeCentroidVel(boids[i]);		
		//headingVel = computeVelocities(boids[i]); 
		//avoidVel = avoidBoids(boids[i]);
		//targetVel = approachTargets(boids[i]);

		int j;
		std::cerr << "########################################################" << std::endl;
		std::cerr << "Boid #" << i << std::endl;
		std::cerr << "########################################################" << std::endl;
		/*
		// Goal #1 - Obstacle Avoidance
		for (j = X; j<=Z; j++)
		{
			newacc[j]  = (avoidWeight * avoidVel[j]);			
			acc[j] = newacc[j];
		}		
		magnitude = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
		if (magnitude > maxAcc)
		{
			for (j = X; j<=Z; j++)
			{
				acc[j] -= newacc[j];
			}
			weight = solveMagnitude(maxAcc, acc, newacc);			
			//cerr << "weight = " << weight << "...\n";
			for (j = X; j<=Z; j++)
			{
				acc[j] += weight * newacc[j];					
			}
			magnitude = maxAcc;
		}
		
		// Goal #2 - Match heading
		if (magnitude < maxAcc)
		{
			for (j = X; j<=Z; j++)
			{
				newacc[j] = headingWeight * (headingVel[j] - boids[i]->vel((Axis)j));
				acc[j] += newacc[j];
			}
			magnitude = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);			
			if (magnitude > maxAcc)
			{ 
				for (j = X; j<=Z; j++)
				{
					acc[j] -= newacc[j];
				}
				weight = solveMagnitude(maxAcc, acc, newacc);
				for (j = X; j<=Z; j++)
				{
					acc[j] += weight * newacc[j];					
				} 
				magnitude = maxAcc; 
			}
		}
		
		// Goal #3 - Approach centroid
		if ((magnitude < maxAcc) && (centroidVel != NULL))
		{
			for (j = X; j<=Z; j++)
			{
				newacc[j] = centroidWeight * centroidVel[j];
				acc[j] += newacc[j];
			}
			magnitude = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
			if (magnitude > maxAcc)
			{
				for (j = X; j<=Z; j++)
				{
					acc[j] -= newacc[j];
				}
				weight = solveMagnitude(maxAcc, acc, newacc);
				for (j = X; j<=Z; j++)
				{
					acc[j] += weight * newacc[j];					
				}
			}
		}
		*/

		/*
		// If acceleration too high, decrease it
		if (magnitude > maxAcc)
		{
			std::cerr << "Acceleration magnitude of " << magnitude << " was too big!\n";
			acc[0] = acc[0] / magnitude * maxAcc;
			acc[1] = acc[1] / magnitude * maxAcc;
			acc[2] = acc[2] / magnitude * maxAcc;
		}
	
		// Goal #4 - approach target(s)
		if (magnitude <= maxAcc)
		{
			for (j = X; j<=Z; j++)
			{
				newacc[j] = targetWeight * (targetVel[j] - boids[i]->vel((Axis)j));
				acc[j] += newacc[j];
			}
			magnitude = sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);			
			std::cerr << "magnitude = " << magnitude << "\n";
			if (magnitude > maxAcc)
			{
				for (j = X; j<=Z; j++)
				{
					acc[j] -= newacc[j];
				}
				weight = solveMagnitude(maxAcc, acc, newacc);
				for (j = X; j<=Z; j++)
				{
					acc[j] += weight * newacc[j];					
				} 
				magnitude = maxAcc; 
			}
		}

		std::cerr << "acc = " << acc[0] << ", " << acc[1] << ", " << acc[2] << "\n";

		for (j = X; j <= Z; j++)
		{
			if ((j == Y) && (ignoreY == true))
			{
				temp[j] = boids[i]->vel((Axis)j);
			} else
			{
				temp[j] = boids[i]->vel((Axis)j) + acc[j];
			}
		}
		
		magnitude = sqrt(temp[0] * temp[0] + temp[1] * temp[1] + temp[2] * temp[2]);

		// Cap the velocity's magnitude to MAXVEL
		if (magnitude > maxVel)
		{
			std::cerr << "Velocity magnitude of " << magnitude << " was too big!\n";
			temp[0] = temp[0] / magnitude * maxVel;
			temp[1] = temp[1] / magnitude * maxVel;
			temp[2] = temp[2] / magnitude * maxVel;
		}
		*/

		Agent* pBoid = boids[i];
		Point* pTarget = targets.front();
		//temp[0] = pTarget->x - boids[i]->pos(X);
		//temp[1] = pTarget->y - boids[i]->pos(Y);
		//temp[2] = pTarget->z - boids[i]->pos(Z);
		temp[0] = pBoid->_v[0], temp[1] = pBoid->_v[1], temp[2] = pBoid->_v[2];

		float ad = 0, tti = 0;
		float p[3] = {pTarget->x, pTarget->y, pTarget->z};
		float pv[3] = {0, 0, 0};
		computeAlphaDerivative(boids[i], p, pv, ad, tti, true);

		std::cerr << "target  = " << pTarget->x << " " << pTarget->y << " " << pTarget->z << "\n";
		std::cerr << "vel = " << boids[i]->_v[0] << " " << boids[i]->_v[1] << " " << boids[i]->_v[2] << "\n";
		std::cerr << "alpha_g = " << ad << ",  tti_g = " << tti << "\n";

		float len = sqrt( temp[0] * temp[0] + temp[1] * temp[1] + temp[2] * temp[2] );
		temp[0] /= len; 
		temp[1] /= len; 
		temp[2] /= len; 
		temp[0] *= pBoid->_v_comf;
		temp[1] *= pBoid->_v_comf;
		temp[2] *= pBoid->_v_comf;

		float cosag = cos(ad);
		float sinag = sin(ad);
		float xx = temp[0], zz = temp[2];
		float zp = zz * cosag - xx * sinag;
		float xp = zz * sinag + xx * cosag;
		temp[0] = xp, temp[2] = zp;

		newpos = boids[i]->matchVel(temp); 
		pBoid->_v[0] = temp[0], pBoid->_v[1] = temp[1], pBoid->_v[2] = temp[2];

		for (j = 0; j <= 2; j++)
		{
			boids[i]->setPos((Axis)j, newpos[j]);
		}
		delete newpos;

		//delete centroidVel;
		//delete headingVel;
		//delete avoidVel;
		//delete targetVel;
	}

	for (i = 0; i < boids.size(); i++)
	{
		boids[i]->tick();
	}
}

void Crowd::computeAlphaDerivative( Agent* boid, float p[3], float pv[3], float& ad, float &tti, bool verbose)
{
	vec3 walker_pos(boid->pos(Z), boid->pos(Y), boid->pos(X));
	vec3 walker_vel(boid->_v[2], boid->_v[1], boid->_v[0]);
	vec3 target_pos(p[2], p[1], p[0]);
	vec3 target_vel(pv[2], pv[1], pv[0]);

	vec2 walker_vel_2d(walker_vel[0], walker_vel[2]);
	vec2 walker_direction_to_target_2d(target_pos[0] - walker_pos[0], target_pos[2] - walker_pos[2]);
	float val_dot_product = walker_direction_to_target_2d * walker_vel_2d;
	float val_cross_product = (walker_direction_to_target_2d ^ walker_vel_2d)[2];

	if (verbose) std::cerr << "walker_pos = " << walker_pos << "\n";
	if (verbose) std::cerr << "walker_vel_2d = " << walker_vel_2d << "\n";
	if (verbose) std::cerr << "walker_direction_to_target_2d = " << walker_direction_to_target_2d << "\n";
	if (verbose) std::cerr << "(walker_direction_to_target_2d ^ walker_vel_2d) = " << (walker_direction_to_target_2d ^ walker_vel_2d) << "\n";

	if (verbose) std::cerr << "target_pos : " << target_pos << "\n";
	if (verbose) std::cerr << "target_vel : " << target_vel << "\n";

	if (val_dot_product >= 0.0)
	{
		// vel is in the same direction to target
		if (verbose) std::cerr << "Same direction to target!!!!!!" << "\n";
		if (val_cross_product >= 0.0)
		{
			if (verbose) std::cerr << "alpha <= 0!!!!!!" << "\n";

			vec3 Vpw = (walker_vel - target_vel);
			if (verbose) std::cerr << "Vpw : " << Vpw << "\n";

			vec3 D = vec3(walker_direction_to_target_2d[0], 0, walker_direction_to_target_2d[1]);
			if (verbose) std::cerr << "D : " << D << "\n";
			float temp = D.Length();

			vec3 k = D.Normalize();
			if (verbose) std::cerr << "k : " << k << "\n";

			vec3 Vconvpw = (Vpw * k) * k;
			if (verbose) std::cerr << "Vconvpw : " << Vconvpw << "\n";

			vec3 Vorthpw = Vpw - Vconvpw;
			if (verbose) std::cerr << "Vorthpw : " << Vorthpw << "\n";

			float Dlen = temp;
			if (verbose) std::cerr << "Dlen : " << Dlen << "\n";

			float VconvLen = Vconvpw.Length();
			if (verbose) std::cerr << "VconvLen : " << VconvLen << "\n";
			if (VconvLen > 0.0f)
				tti = Dlen / VconvLen;
			else
				tti = 1000000000.0f;

			float VorthLen = Vorthpw.Length();
			if (verbose) std::cerr << "VorthLen : " << VorthLen << "\n";

			if (fabs(Dlen - VconvLen) > 0.0f)
				ad = atan2( VorthLen, (Dlen - VconvLen) );
			else
				ad = 0.0f;

			vec2 Vorth_2d(Vorthpw[0], Vorthpw[2]);
			float val_cross_product_of_Vw_and_Vorth = (walker_vel_2d ^ Vorth_2d)[2];
			if (verbose) std::cerr << "val_cross_product_of_Vw_and_Vorth : " << val_cross_product_of_Vw_and_Vorth << "\n";
			//if (val_cross_product_of_Vw_and_Vorth >= 0.0)
			if (target_vel.Length() < walker_vel.Length())
				ad = -fabs(ad);
			else
				ad = fabs(ad);
		}
		else
		{
			if (verbose) std::cerr << "alpha > 0!!!!!!" << "\n";

			vec3 Vpw = (walker_vel - target_vel);
			if (verbose) std::cerr << "Vpw : " << Vpw << "\n";

			vec3 D = vec3(walker_direction_to_target_2d[0], 0, walker_direction_to_target_2d[1]);
			if (verbose) std::cerr << "D : " << D << "\n";
			float temp = D.Length();

			vec3 k = D.Normalize();
			if (verbose) std::cerr << "k : " << k << "\n";

			vec3 Vconvpw = (Vpw * k) * k;
			if (verbose) std::cerr << "Vconvpw : " << Vconvpw << "\n";

			vec3 Vorthpw = Vpw - Vconvpw;
			if (verbose) std::cerr << "Vorthpw : " << Vorthpw << "\n";

			float Dlen = temp;
			if (verbose) std::cerr << "Dlen : " << Dlen << "\n";

			float VconvLen = Vconvpw.Length();
			if (verbose) std::cerr << "VconvLen : " << VconvLen << "\n";
			if (VconvLen > 0.0f)
				tti = Dlen / VconvLen;
			else
				tti = 1000000000.0f;

			float VorthLen = Vorthpw.Length();
			if (verbose) std::cerr << "VorthLen : " << VorthLen << "\n";

			if (fabs(Dlen - VconvLen) > 0.0f)
				ad = atan2( VorthLen, (Dlen - VconvLen) );
			else
				ad = 0.0f;

			vec2 Vorth_2d(Vorthpw[0], Vorthpw[2]);
			float val_cross_product_of_Vw_and_Vorth = (walker_vel_2d ^ Vorth_2d)[2];
			if (verbose) std::cerr << "val_cross_product_of_Vw_and_Vorth : " << val_cross_product_of_Vw_and_Vorth << "\n";
			//if (val_cross_product_of_Vw_and_Vorth >= 0.0)
			if (target_vel.Length() < walker_vel.Length())
				ad = fabs(ad);
			else
				ad = -fabs(ad);
		}
	}
	else
	{
		// in opposite direction
		if (verbose) std::cerr << "Opposite direction to target!!!!!!" << "\n";
		if (val_cross_product >= 0.0)
		{
			if (verbose) std::cerr << "alpha <= 0!!!!!!" << "\n";

			vec3 Vpw = (walker_vel - target_vel);
			if (verbose) std::cerr << "Vpw : " << Vpw << "\n";

			vec3 D = vec3(walker_direction_to_target_2d[0], 0, walker_direction_to_target_2d[1]);
			if (verbose) std::cerr << "D : " << D << "\n";
			float temp = D.Length();

			vec3 k = D.Normalize();
			if (verbose) std::cerr << "k : " << k << "\n";

			vec3 Vconvpw = (Vpw * k) * k;
			if (verbose) std::cerr << "Vconvpw : " << Vconvpw << "\n";

			vec3 Vorthpw = Vpw - Vconvpw;
			if (verbose) std::cerr << "Vorthpw : " << Vorthpw << "\n";

			float Dlen = temp;
			if (verbose) std::cerr << "Dlen : " << Dlen << "\n";

			float VconvLen = Vconvpw.Length();
			if (verbose) std::cerr << "VconvLen : " << VconvLen << "\n";
			if (VconvLen > 0.0f)
				tti = Dlen / VconvLen;
			else
				tti = 1000000000.0f;

			float VorthLen = Vorthpw.Length();
			if (verbose) std::cerr << "VorthLen : " << VorthLen << "\n";

			if (fabs(Dlen - VconvLen) > 0.0f)
				ad = atan2( VorthLen, (Dlen - VconvLen) );
			else
				ad = 0.0f;

			vec2 Vorth_2d(Vorthpw[0], Vorthpw[2]);
			float val_cross_product_of_Vw_and_Vorth = (walker_vel_2d ^ Vorth_2d)[2];
			if (verbose) std::cerr << "val_cross_product_of_Vw_and_Vorth : " << val_cross_product_of_Vw_and_Vorth << "\n";
			//if (val_cross_product_of_Vw_and_Vorth >= 0.0)
			if (target_vel.Length() < walker_vel.Length())
				ad = -fabs(ad);
			else
				ad = fabs(ad);
		}
		else
		{
			if (verbose) std::cerr << "alpha > 0!!!!!!" << "\n";

			vec3 Vpw = (walker_vel - target_vel);
			if (verbose) std::cerr << "Vpw : " << Vpw << "\n";

			vec3 D = vec3(walker_direction_to_target_2d[0], 0, walker_direction_to_target_2d[1]);
			if (verbose) std::cerr << "D : " << D << "\n";
			float temp = D.Length();

			vec3 k = D.Normalize();
			if (verbose) std::cerr << "k : " << k << "\n";

			vec3 Vconvpw = (Vpw * k) * k;
			if (verbose) std::cerr << "Vconvpw : " << Vconvpw << "\n";

			vec3 Vorthpw = Vpw - Vconvpw;
			if (verbose) std::cerr << "Vorthpw : " << Vorthpw << "\n";

			float Dlen = temp;
			if (verbose) std::cerr << "Dlen : " << Dlen << "\n";

			float VconvLen = Vconvpw.Length();
			if (verbose) std::cerr << "VconvLen : " << VconvLen << "\n";
			if (VconvLen > 0.0f)
				tti = Dlen / VconvLen;
			else
				tti = 1000000000.0f;

			float VorthLen = Vorthpw.Length();
			if (verbose) std::cerr << "VorthLen : " << VorthLen << "\n";

			if (fabs(Dlen - VconvLen) > 0.0f)
				ad = atan2( VorthLen, (Dlen - VconvLen) );
			else
				ad = 0.0f;

			vec2 Vorth_2d(Vorthpw[0], Vorthpw[2]);
			float val_cross_product_of_Vw_and_Vorth = (walker_vel_2d ^ Vorth_2d)[2];
			if (verbose) std::cerr << "val_cross_product_of_Vw_and_Vorth : " << val_cross_product_of_Vw_and_Vorth << "\n";
			//if (val_cross_product_of_Vw_and_Vorth >= 0.0)
			if (target_vel.Length() < walker_vel.Length())
				ad = fabs(ad);
			else
				ad = -fabs(ad);
		}
	}


	return;
}

// This will return true if boid1 and boid2 are neighbors. This is determined by
// checking if their distance squared is less than or equal to some amount
bool Crowd::areNeighbors(Agent *boid1, Agent *boid2)
{
	if (boid1->id() == boid2->id()) return false;

	return ((
		(boid1->pos(X) - boid2->pos(X))*(boid1->pos(X) - boid2->pos(X)) + 
		(boid1->pos(Y) - boid2->pos(Y))*(boid1->pos(Y) - boid2->pos(Y)) + 
		(boid1->pos(Z) - boid2->pos(Z))*(boid1->pos(Z) - boid2->pos(Z))) <= neighborDist * neighborDist);
}

// Return the square distance between two boids
float Crowd::getDist2(Agent *boid1, Agent *boid2)
{
	return ((
		(boid1->pos(X) - boid2->pos(X))*(boid1->pos(X) - boid2->pos(X)) + 
		(boid1->pos(Y) - boid2->pos(Y))*(boid1->pos(Y) - boid2->pos(Y)) + 
		(boid1->pos(Z) - boid2->pos(Z))*(boid1->pos(Z) - boid2->pos(Z))));
}

// Compute the location of the centroid for neighboring boids
// Note: we get rounding errors here
float *Crowd::computeCentroidVel(Agent *boid)
{
	float *totals = new float[3];
	float *newtotals;
	int numNeighbors = 0;
	int i;
	for (i = X; i <= Z; i++) totals[i] = 0;

	for (i = 0; i < boids.size(); i++)
	{
		if (areNeighbors(boid, boids[i]))
		{
			numNeighbors++;
			totals[X] += boids[i]->pos(X);
			totals[Y] += boids[i]->pos(Y);
			totals[Z] += boids[i]->pos(Z);
		}
	}
	if (numNeighbors)
	{
		totals[X] /= numNeighbors;
		totals[Y] /= numNeighbors;
		totals[Z] /= numNeighbors;
	} else
	{
		return NULL;
	}

	newtotals = boid->computeVel(totals);

	return newtotals;
}

//We assume that totals is already initialized to an
//array of at least size 3.
void Crowd::getCentroid(float *totals)
{
	int numNeighbors = 0;

	int i;
	for (i = X; i <= Z; i++) totals[i] = 0;

	for (i = 0; i < boids.size(); i++)
	{
		numNeighbors++;
		totals[X] += boids[i]->pos(X);
		totals[Y] += boids[i]->pos(Y);
		totals[Z] += boids[i]->pos(Z);
	}
	if (numNeighbors)
	{
		totals[X] /= numNeighbors;
		totals[Y] /= numNeighbors;
		totals[Z] /= numNeighbors;
	}
}

// Compute the total velocity that a boid should move for object avoidance
float *Crowd::avoidBoids(Agent *boid)
{
	float *totals = new float[3];
	float dist2;
	int numNeighbors = 0;
	int i;
	for (i = X; i <= Z; i++) totals[i] = 0;

	for (i = 0; i < boids.size(); i++)
	{
		dist2 = getDist2(boid, boids[i]);
		if (dist2 < ((avoidDist + boid->radius + boids[i]->radius) * (avoidDist + boid->radius + boids[i]->radius)))
		{
			//FIXME...should not include radii in totals
			// should figure out portion of radius going in each direction based on orientations
			// this will make collisions more realistic (right now, they bounce off too quickly)
			// We should do the same for obstacles
			//
			//For now this is not a problem, but this is radius dependent
			totals[X] += boid->pos(X) - boids[i]->pos(X); // - 2 * boid->radius;
			totals[Y] += boid->pos(Y) - boids[i]->pos(Y); // - 2 * boid->radius;
			totals[Z] += boid->pos(Z) - boids[i]->pos(Z); // - 2 * boid->radius;
		}
	}
	
	for (i = 0; i < obstacles.size(); i++)
	{
		dist2 = (boid->pos(X) - obstacles[i]->x) * (boid->pos(X) - obstacles[i]->x) + 
				(boid->pos(Y) - obstacles[i]->y) * (boid->pos(Y) - obstacles[i]->y) + 
				(boid->pos(Z) - obstacles[i]->z) * (boid->pos(Z) - obstacles[i]->z);	
		if (dist2 < (boid->radius + obstacles[i]->radius + avoidDist) * (boid->radius + obstacles[i]->radius + avoidDist))
		{			
			totals[X] += 10*(boid->pos(X) - obstacles[i]->x); //- obstacles[i]->radius - boid->radius;
			totals[Y] += 10*(boid->pos(Y) - obstacles[i]->y);		
			totals[Z] += 10*(boid->pos(Z) - obstacles[i]->z);
		}
	}

	return totals;
}

float *Crowd::approachTargets(Agent *boid)
{
	float *totals = new float[3];
	float dist;
	float targetVel[3];

	int i;
	for (i = 0; i < 3; i++) totals[i] = 0;
	
	for (i = 0; i <= 2; i++)
	{
		targetVel[i] = maxVel / 2.0f;
	}
	
	for (i = 0; i < targets.size(); i++)
	{
		dist = sqrt(((targets[i]->x - boid->pos(X)) * (targets[i]->x - boid->pos(X))) + ((targets[i]->y - boid->pos(Y)) * (targets[i]->y - boid->pos(Y))) + ((targets[i]->z - boid->pos(Z)) * (targets[i]->z - boid->pos(Z))));
		if (dist * dist > 0.0f)
		{
			totals[0] += ((targets[i]->x - boid->pos(X)) / dist * targetVel[0]);
			totals[1] += ((targets[i]->y - boid->pos(Y)) / dist * targetVel[1]);
			totals[2] += ((targets[i]->z - boid->pos(Z)) / dist * targetVel[2]);
		}
		else
		{
			totals[0] += 0.0f;
			totals[1] += 0.0f;
			totals[2] += 0.0f;
		}
	}

	std::cerr << "target totals = " << totals[0] << " " << totals[1] << " " << totals[2] << "\n";
	std::cerr << "vel = " << boid->vel(X)  << boid->vel(Y)  << boid->vel(Z) << "\n";

	return totals;
}

/*
// Compute the average velocity of all neighboring boids
// this uses a weighted average where boids that are farther away have exponentially less influence
float *computeVelocities(Boid *boid)
{
	float *totals = new float[3];
	float denom = 0;
	float dist;
	double weight = 0;
	for (int i = X; i <= Z; i++) totals[i] = 0;

	for (i = 0; i < boids.size(); i++)
	{
		if (areNeighbors(boid, boids[i]))
		{
			dist = sqrt(getDist2(boid, boids[i]));

			if (dist < 0.1)
			{
				weight = exp(10/0.1);
			} else
			{
				weight = exp(10/dist);
			}
			cerr << "weight="<<weight<<"\n";
			denom += weight;
			totals[X] += weight * boids[i]->vel(X);
			totals[Y] += weight * boids[i]->vel(Y);
			totals[Z] += weight * boids[i]->vel(Z);
		}
	}
	if (denom)
	{
		totals[X] /= denom;
		totals[Y] /= denom;
		totals[Z] /= denom;
	}

	return totals;
}
*/

// Compute the average velocity of all neighboring boids
float *Crowd::computeVelocities(Agent *boid)
{
	float *totals = new float[3];
	int numNeighbors = 0;
	int i;
	for (i = X; i <= Z; i++) totals[i] = 0;

	for (i = 0; i < boids.size(); i++)
	{
		if (areNeighbors(boid, boids[i]))
		{
			numNeighbors++;
			totals[X] += boids[i]->vel(X);
			totals[Y] += boids[i]->vel(Y);
			totals[Z] += boids[i]->vel(Z);
		}
	}
	if (numNeighbors)
	{
		totals[X] /= numNeighbors;
		totals[Y] /= numNeighbors;
		totals[Z] /= numNeighbors;
	}
	return totals;
}

//Here, we solve a quadratic to determine the weight for newacc so
//that the magnitude of acc equals magnitude

//[(x1 - kx2)^2 + (y1 - ky2)^2 + (z1 - kz2)^2] = magnitude^2
//
//solve for k
float Crowd::solveMagnitude(float magnitude, float acc[3], float newacc[3])
{
	float weight;
	float total;
	float a,b,c;

	total = sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
	if (total < 0.01) //If current acceleration is really small, then we will ignore it
	{
		total = sqrt(newacc[0]*newacc[0] + newacc[1]*newacc[1] + newacc[2]*newacc[2]);
		//if (total != 0)
		if (fabs(total) > 0.0f)
		{			
			weight = magnitude / total;
			//cerr << "weight = " << weight << ", total = " << total << ", magnitude = " << magnitude << "\n";
		} else
		{
			weight = 1;
		}
	}
	else
	{
		c = (acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]) - (magnitude * magnitude);
		b = -2 * (acc[0] * newacc[0] + acc[1] * newacc[1] + acc[2] * newacc[2]);
		a = (newacc[0] * newacc[0] + newacc[1] * newacc[1] + newacc[2] * newacc[2]);

		//cerr << " a= " << a << " b = " << b << " c = " << c;
		weight = (-b + sqrt(b*b - 4*a*c))/(2*a);

		if (weight > 1) weight = 1;	
	}

	//cerr << "weight = " << weight << " discriminant = " << sqrt(b*b - 4*a*c) << "\n";
	//cerr << "acc = " << acc[0] << ", " << acc[1] << ", " << acc[2] << "...\n";
	//cerr << "newacc = " << newacc[0] << ", " << newacc[1] << ", " << newacc[2] << "...\n";

	return weight;
};

Crowd::~Crowd()
{
	clear();
}

void Crowd::clear()
{
	int i;
	for (i = 0; i < targets.size(); i++)
	{
		delete targets[i];
	}
	targets.clear();

	for (i = 0; i < boids.size(); i++)
	{
		delete boids[i];
	}
	boids.clear();
	
	for (i = 0; i < obstacles.size(); i++)
	{
		delete obstacles[i];
	}
	obstacles.clear();

	for (i = 0; i < goals.size(); i++)
	{
		delete goals[i];
	}
}

float Crowd::computeTau( float tti, float ad )
{
	float const a = 0.0;
	float const b = 0.6;
	float const c = 1.5;
	if (ad < 0.0)
		return a - b * std::pow(tti, -c);
	else
		return a + b * std::pow(tti, -c);
}
